/* 
 * File:   splam.h
 * Author: hadrien
 *
 * Created on 4 mars 2012, 04:15
 */

#ifndef SPLAM_H
#define	SPLAM_H

#include <mrpt/base.h>
#include <mrpt/slam.h>
#include <mrpt/gui.h>
#include "Win3D.h"
#include "global.h"
#include "Robot.h"

void createLandmarkMap(mrpt::slam::CLandmarksMap &landmarkMap);
void outputError(
        const mrpt::slam::CLandmarksMap                                 &landmarkMap, 
        const std::vector<mrpt::poses::TPoint2D>                        &LMs, 
        std::map<unsigned int, mrpt::slam::CLandmark::TLandmarkID>      &LM_IDs
);
void saveStep(
        const mrpt::poses::CPosePDFGaussian                             &robotPose, 
        mrpt::math::CMatrixDouble                                       &fullCov, 
        const mrpt::slam::CRangeBearingKFSLAM2D                         &mapping, 
        const std::size_t                                               &step
);

#endif	/* SPLAM_H */

